'''
####
Date : 2024-10-17
Notes: 机器人控制
Explain: 
    1. 串口通讯
    2. 运动控制函数接口
####
'''

# -*- coding: utf-8 -*-
import os
import struct
import time
import sqlite3
import serial
import config
import logging
import math
from serial import Serial
from threading import Thread

robot_logger=logging.getLogger("robot_control")
robot_logger.setLevel(logging.DEBUG)

### 气泵PWM值
AIR_PUMP_OFF_PWM_VALUE = 0
AIR_PUMP_BLOWING_PWM_VALUE = 500
AIR_PUMP_SUCTION_PWM_VALUE = 1000

# 电磁阀PWM值
VALVE_OFF_PWM_VALUE = 65
VALVE_ON_PWM_VALUE = 40 



class robot_Control():

    def __init__(self) -> None:
        self.status_global = None
        self.status_global_ = None
        self.status_global_3 = None
        self.default_speed = 2000
        # 这里来表示机器人类型。0：代表无机器人，1:代表是六轴，2：代表是四轴
        self.robot_type_number = 0 
        self.robot_type_number_2 = 0
        self.robot_type_number_3 = 0

        self.serial_com1 = None
        self.serial_com2 = None
        self.serial_com3 = None
        self.thread_com1 = None
        self.thread_com2 = None
        self.thread_com3 = None

        self.bo_out =0x00 

        # 气泵PWM值
        self.pump_pwm_values = [
            AIR_PUMP_SUCTION_PWM_VALUE,
            AIR_PUMP_BLOWING_PWM_VALUE,
            AIR_PUMP_OFF_PWM_VALUE
            ]
        # 电磁阀PWM值
        self.valve_pwm_values = [
            VALVE_OFF_PWM_VALUE,
            VALVE_ON_PWM_VALUE
            ]

        # 定义RTU服务内容
        self.rtu_slave = 0
        self.rut_en = 0

        ########################数据库存储状态############################
        filename = 'ktr6.db'
        filename_ = 'ktr4.db'
        filename_3 = 'ktr_usb.db'
        # file_dir = "/home/teamhd/factory/data/"
        current_dir = os.path.dirname(os.path.abspath(__file__))
        data_file_path = os.path.join(current_dir, "..", "data")

        # 使用路径模块连接
        file_path = os.path.join(data_file_path, filename)
        file_path_ = os.path.join(data_file_path, filename_)
        file_path_3 = os.path.join(data_file_path, filename_3)
        
        self.conn = sqlite3.connect(file_path,timeout=10,check_same_thread=False)
        self.conn_  = sqlite3.connect(file_path_,timeout=10,check_same_thread=False)
        self.conn_3 = sqlite3.connect(file_path_3,timeout=10,check_same_thread=False)
        
        self._init_serial()

    def bo_count(self, BO, bo_num, value):
        # BO 是一个8位的16进制数
        # bo_bit 是第几个IO
        # value 是DO的值
        # 将DO状态上传IOA服务器

        try:
            bit_num = 0b00000000
            if value == 0:
                if bo_num == 0:
                    bit_num = 0b11111110
                elif bo_num == 1:
                    bit_num = 0b11111101
                elif bo_num == 2:
                    bit_num = 0b11111011
                elif bo_num == 3:
                    bit_num = 0b11110111
                elif bo_num == 4:
                    bit_num = 0b11101111
                elif bo_num == 5:
                    bit_num = 0b11011111
                elif bo_num == 6:
                    bit_num = 0b10111111
                elif bo_num == 7:
                    bit_num = 0b01111111
                BO = BO & bit_num
            elif value == 1:
                if bo_num == 0:
                    bit_num = 0b00000001
                elif bo_num == 1:
                    bit_num = 0b00000010
                elif bo_num == 2:
                    bit_num = 0b00000100
                elif bo_num == 3:
                    bit_num = 0b00001000
                elif bo_num == 4:
                    bit_num = 0b00010000
                elif bo_num == 5:
                    bit_num = 0b00100000
                elif bo_num == 6:
                    bit_num = 0b01000000
                elif bo_num == 7:
                    bit_num = 0b10000000
                BO = BO | bit_num
            return BO

        except Exception as e:
            print(e)

    def run(self):
        while True:
            pass

    def _init_serial(self):
        self.serial_com1 = None  
        try:  
            self.serial_com1 = serial.Serial("/dev/ttyS3", baudrate=115200, timeout=1)
            time.sleep(0.05) 
        except serial.SerialException as e:  
            print(f"Failed to open port /dev/ttyS3: {e}")  

        if self.serial_com1:  
            self.thread_update_ = Thread(target=self.task_com1)  
            self.thread_update_.start()

        self.serial_com2 = None  
        try:  
            self.serial_com2 = serial.Serial("/dev/ttyS4", baudrate=115200, timeout=1) 
            time.sleep(0.05)  
        except serial.SerialException as e:  
            print(f"Failed to open port /dev/ttyS4: {e}")  
  
        if self.serial_com2:  
            self.thread_update = Thread(target=self.task_com2)  
            self.thread_update.start()

        # self.serial_com3 = None
        # ports = ['/dev/ttyUSB7', '/dev/ttyUSB8', '/dev/ttyUSB9', '/dev/ttyUSB10']
        # try:  
        #     self.serial_com3 = self.try_serial_ports(ports)
        # except serial.SerialException as e:  
        #     print(f"Failed to open port /dev/ttyUSB: {e}")  
        
        # if self.serial_com3:  
        #     self.thread_update_3 = Thread(target=self.task_com3)  
        #     self.thread_update_3.start()

    def try_serial_ports(self, ports):  
        for port in ports:  
            try:  
                ser = Serial(port, 115200, timeout=0.5)
                time.sleep(0.91)
                print(f"Successfully opened {port}")  
                return ser  
            except serial.serialutil.SerialException as e:  
                print(f"Failed to open {port}: {e}")  
                time.sleep(0.1)  # 等待一秒再尝试下一个端口  
        return None  

    def task_com1(self,):
        type_flag_count = 0   
        while True:
            try:
                if self.serial_com1 is not None:
                    gcode = f"?\r\n"
                    self.send_msg(gcode, com_flag=1)
                    time.sleep(0.1)
                    try:
                        ### 从串口读取数据 
                        data = self.serial_com1.read_all()
                        # print(f"---com1 recv data {data}---")
                    except serial.SerialException as e:  
                        print(f"serial_com1.read_all: {e}")
                    
                    ### 如果第一次建立通讯，就询问一下机器人的类型
                    if data != b"":
                        type_flag_count = 0
                        ### 进来第一次询问
                        if self.robot_type_number == 0:  
                            self.robot_type_update(com_flag=1)

                    else:
                        type_flag_count +=1

                        if type_flag_count == 10:
                            self.robot_type_number = 0
                            type_flag_count = 0
                        continue

                    if data != b"":
                        self.Kata_status2json(data)
                        self.set_robot_state(self.status_global["state"])
                
            except Exception as e:
                robot_logger.error(e,exc_info=True)

    def task_com2(self,):
        type_flag_count = 0
        while True:
            try:
                if self.serial_com2 is not None:
                    gcode = f"?\r\n"
                    self.send_msg(gcode, com_flag=2)
                    time.sleep(0.1)
                    try:
                        ### 从串口读取数据 
                        data = self.serial_com2.read_all()
                        # print(f"---com2 recv data {data}---")
                    except serial.SerialException as e:  
                        print(f"serial_com2.read_all: {e}") 
                    

                    ### 如果第一次建立通讯，就询问一下机器人的类型
                    if data != b"":
                        type_flag_count = 0
                        ### 进来第一次询问
                        if self.robot_type_number_2 == 0:  
                            self.robot_type_update_2(com_flag=2)

                    else:
                        type_flag_count +=1
                        if type_flag_count == 10:
                            self.robot_type_number_2 = 0
                            type_flag_count = 0
                        continue

                    if data != b"":
                        self.Kata_status2json_2(data)
                        self.set_robot_state_(self.status_global_["state"])
            except Exception as e:
                robot_logger.error(e,exc_info=True)
    
    #### USB串口
    def task_com3(self,):
        type_flag_count = 0
        while True:
            try:
                if self.serial_com3 is not None:
                    gcode = f"?\r\n"
                    self.send_msg(gcode, com_flag=3)
                    time.sleep(0.15)
                    try:
                        ### 从串口读取数据 
                        data = self.serial_com3.read_all()
                        # print(f"---com3 recv data {data}---")
                    except serial.SerialException as e:  
                        print(f"serial_com3.read_all: {e}") 
                    

                    ### 如果第一次建立通讯，就询问一下机器人的类型
                    if data != b"":
                        type_flag_count = 0
                        ### 进来第一次询问
                        if self.robot_type_number_3 == 0:  
                            self.robot_type_update_3(com_flag=3)

                    else:
                        type_flag_count +=1
                        if type_flag_count == 10:
                            self.robot_type_number_3 = 0
                            type_flag_count = 0
                        continue

                    if data != b"":
                        self.Kata_status2json_3(data)
                        self.set_robot_state_3(self.status_global_3["state"])
            except Exception as e:
                robot_logger.error(e,exc_info=True)

    def parse_received_data(self, data):
            """Parse received data and return a dictionary."""
            try:
                receive = str(data, "UTF-8")
                lower = receive.find('<')
                if lower != -1:
                    upper = receive.find('>', lower)
                    if upper != -1:
                        status_data = receive[lower + 1:upper]
                        return status_data.split(',')
                return None
            except UnicodeDecodeError as e:
                robot_logger.error(f"Unicode decode error: {e}", exc_info=True)
                return None

    def Kata_status2json(self, data):
        try:
            status_date = None
            status =None
            status_date = self.parse_received_data(data)
           
            if status_date !=None:
               
                status  = {
                        "state":status_date[0],
                        "angle":[
                            float(status_date[5]),
                            float(status_date[6]),
                            float(status_date[7]),
                            float(status_date[1].split(':')[1]),
                            float(status_date[2]),
                            float(status_date[3]),
                            
                        ],
                        "coordinate":[
                            float(status_date[8].split(':')[1]),
                            float(status_date[9]),
                            float(status_date[10]),
                            float(status_date[11]),
                            float(status_date[12]),
                            float(status_date[13]),
                            float(status_date[4]),
                        ],
                        "pwm":[
                            float(status_date[14].split(':')[1]),
                            float(status_date[15].split(':')[1]),
                        ],
                        # "mode":[
                        #     int(status_date[16].split(':')[1]),],
                        "robot_type":self.robot_type_number
                    }
            
            if status != None:
                self.status_global = status
                self.send_tcp_data(status)
            return status

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def Kata_status2json_2(self,data):
        try:
            status_date = None
            status =None
            status_date = self.parse_received_data(data)
            
            if status_date !=None:

                status  = {
                        "state":status_date[0],
                        "angle":[
                            float(status_date[5]),
                            float(status_date[6]),
                            float(status_date[7]),
                            float(status_date[1].split(':')[1]),
                            float(status_date[2]),
                            float(status_date[3]),
                        ],
                        "coordinate":[
                            float(status_date[8].split(':')[1]),
                            float(status_date[9]),
                            float(status_date[10]),
                            float(status_date[11]),
                            float(status_date[12]),
                            float(status_date[13]),
                            float(status_date[4]),
                        ],
                        "pwm":[
                            float(status_date[14].split(':')[1]),
                            float(status_date[15].split(':')[1]),
                        ],
                        # "mode":[
                        #     int(status_date[16].split(':')[1]),],
                        "robot_type":self.robot_type_number_2
                    }
            
            if status != None:
                self.status_global_ = status
                self.send_tcp_data_2(status)
            return status

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def Kata_status2json_3(self,data):
        try:
            status_date = None
            status =None
            status_date = self.parse_received_data(data)
            
            if status_date !=None:
                # status_date = status_date.split(',')
                status  = {
                        "state":status_date[0],
                        "angle":[
                            float(status_date[5]),
                            float(status_date[6]),
                            float(status_date[7]),
                            float(status_date[1].split(':')[1]),
                            float(status_date[2]),
                            float(status_date[3]),
                        ],
                        "coordinate":[
                            float(status_date[8].split(':')[1]),
                            float(status_date[9]),
                            float(status_date[10]),
                            float(status_date[11]),
                            float(status_date[12]),
                            float(status_date[13]),
                            float(status_date[4]),
                        ],
                        "pwm":[
                            float(status_date[14].split(':')[1]),
                            float(status_date[15].split(':')[1]),
                        ],
                        # "mode":[
                        #     int(status_date[16].split(':')[1]),],
                        "robot_type":self.robot_type_number_3
                    }
            
            if status != None:
                self.status_global_3 = status
                self.send_tcp_data_3(status)
            return status

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def set_robot_state(self,data):
        try :
            c = self.conn.cursor()
            state = data
            en = c.execute("SELECT EN,DATA1,DATA2 from Robot_state where ID=1")
            data_en = en.fetchall()

            # 检测是否有G代码要执行
            if data_en[0][0] == 1:
                gcode = c.execute("SELECT GCODE from Robot_state where ID=1")
                data_gcode = gcode.fetchall()
                print(f"gcode is {data_gcode[0][0]}")
                self.send_msg(data_gcode[0][0], com_flag= 1)
                c.execute("UPDATE Robot_state set EN = 0 where ID=1")
                # self.conn.commit() 
            self.rtu_slave = data_en[0][1]
            self.rut_en = data_en[0][2]  # 更新服务状态
            # 将机器人状态更新到数据库里。
            if state=="Idle":
                c.execute("UPDATE Robot_state set STATE = 'Idle' where ID=1")
            elif state=="Alarm":
                c.execute("UPDATE Robot_state set STATE = 'Alarm' where ID=1")
            elif state=="Home":
                c.execute("UPDATE Robot_state set STATE = 'Home' where ID=1")
            elif state=="Run":
                c.execute("UPDATE Robot_state set STATE = 'Run' where ID=1")
            self.conn.commit() 
        except Exception as e:
            print(f"set_robot_state error is {e}")

    def set_robot_state_(self,data):
        try :
            c = self.conn_.cursor()
            state = data
            en = c.execute("SELECT EN,DATA1,DATA2 from Robot_state2 where ID=1")
            data_en = en.fetchall()
            # 检测是否有G代码要执行
            if data_en[0][0] == 1:
                gcode = c.execute("SELECT GCODE from Robot_state2 where ID=1")
                
                data_gcode = gcode.fetchall()
                self.send_msg(data_gcode[0][0], com_flag= 2)
                c.execute("UPDATE Robot_state2 set EN = 0 where ID=1")
                # self.conn.commit() 
            self.rtu_slave = data_en[0][1]
            self.rut_en = data_en[0][2]  # 更新服务状态
            # 将机器人状态更新到数据库里。
            # 更新状态  
            # if state in ["Idle", "Alarm", "Home", "Run"]:  
            #     print(6)
            #     c.execute("UPDATE Robot_state2 set STATE = ? where ID=1 ", (state))
            if state=="Idle":
                c.execute("UPDATE Robot_state2 set STATE = 'Idle' where ID=1")
            elif state=="Alarm":
                c.execute("UPDATE Robot_state2 set STATE = 'Alarm' where ID=1")
            elif state=="Home":
                c.execute("UPDATE Robot_state2 set STATE = 'Home' where ID=1")
            elif state=="Run":
                c.execute("UPDATE Robot_state2 set STATE = 'Run' where ID=1") 
            self.conn_.commit() 
        except Exception as e:
            print(f"set_robot_state_2 error is {e}")

    def set_robot_state_3(self,data):
        try :
            c = self.conn_3.cursor()
            state = data
            en = c.execute("SELECT EN,DATA1,DATA2 from Robot_state3 where ID=1")
            data_en = en.fetchall()
            # 检测是否有G代码要执行
            if data_en[0][0] == 1:
                gcode = c.execute("SELECT GCODE from Robot_state3 where ID=1")
                data_gcode = gcode.fetchall()
                self.send_msg(data_gcode[0][0], com_flag= 3)
                c.execute("UPDATE Robot_state3 set EN = 0 where ID=1")
                # self.conn.commit() 
            self.rtu_slave = data_en[0][1]
            self.rut_en = data_en[0][2]  # 更新服务状态
            # 将机器人状态更新到数据库里。
            # 更新状态  
            # if state in ["Idle", "Alarm", "Home", "Run"]:  
            #     print(6)
            #     c.execute("UPDATE Robot_state2 set STATE = ? where ID=1 ", (state))
            if state=="Idle":
                c.execute("UPDATE Robot_state3 set STATE = 'Idle' where ID=1")
            elif state=="Alarm":
                c.execute("UPDATE Robot_state3 set STATE = 'Alarm' where ID=1")
            elif state=="Home":
                c.execute("UPDATE Robot_state3 set STATE = 'Home' where ID=1")
            elif state=="Run":
                c.execute("UPDATE Robot_state3 set STATE = 'Run' where ID=1") 
            self.conn_3.commit() 
        except Exception as e:
            print(f"set_robot_state_3 error is {e}")

    
    #### 数据---TCP
    def send_tcp_data(self,data):
        try:
            # if self.robot_type_number == 1:
            j0  = data["angle"][0]
            j1  = data["angle"][1]
            j2  = data["angle"][2]
            j3  = data["angle"][3]
            j4  = data["angle"][4]
            j5  = data["angle"][5]
            j6  = data["coordinate"][6]
            s1  = data["state"]

            c0 = data["coordinate"][0]
            c1 = data["coordinate"][1]
            c2 = data["coordinate"][2]
            c3 = data["coordinate"][3]
            c4 = data["coordinate"][4]
            c5 = data["coordinate"][5]
            robot_type = data["robot_type"]

            
            ###将MS数据存入字典
            config.set_value("j0",j0) 
            config.set_value("j1",j1)
            config.set_value("j2",j2)
            config.set_value("j3",j3)
            config.set_value("j4",j4)
            config.set_value("j5",j5)
            config.set_value("FO[1]",value=j6)

            config.set_value("c0",c0)
            config.set_value("c1",c1)
            config.set_value("c2",c2)
            config.set_value("c3",c3)
            config.set_value("c4",c4)
            config.set_value("c5",c5)

            config.set_value("s1",s1)
            config.set_value("robot_type", robot_type)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def send_tcp_data_2(self,data):
        try:
            j7  = data["angle"][0]
            j8  = data["angle"][1]
            j9  = data["angle"][2]
            j10  = data["angle"][3]
            j11  = data["angle"][4]
            j12  = data["angle"][5]
            j13  = data["coordinate"][6]
            s2  = data["state"]

            c6 = data["coordinate"][0]
            c7 = data["coordinate"][1]
            c8 = data["coordinate"][2]
            c9 = data["coordinate"][3]
            c10 = data["coordinate"][4]
            c11 = data["coordinate"][5]
            robot_type2 = data["robot_type"]

            config.set_value("j7",j7) 
            config.set_value("j8",j8)
            config.set_value("j9",j9)
            config.set_value("j10",j10)
            config.set_value("j11",j11)
            config.set_value("j12",j12)
            config.set_value("FO[2]",value=j13)

            config.set_value("c6",c6)
            config.set_value("c7",c7)
            config.set_value("c8",c8)
            config.set_value("c9",c9)
            config.set_value("c10",c10)
            config.set_value("c11",c11)
            config.set_value("s2",s2)
            config.set_value("robot_type2", robot_type2)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def send_tcp_data_3(self,data):
        try:
            j14  = data["angle"][0]
            j15  = data["angle"][1]
            j16  = data["angle"][2]
            j17  = data["angle"][3]
            j18  = data["angle"][4]
            j19  = data["angle"][5]
            j20  = data["coordinate"][6]
            s3  = data["state"]

            c12 = data["coordinate"][0]
            c13 = data["coordinate"][1]
            c14 = data["coordinate"][2]
            c15 = data["coordinate"][3]
            c16 = data["coordinate"][4]
            c17 = data["coordinate"][5]
            robot_type3 = data["robot_type"]

            config.set_value("j14",j14) 
            config.set_value("j15",j15)
            config.set_value("j16",j16)
            config.set_value("j17",j17)
            config.set_value("j18",j18)
            config.set_value("j19",j19)
            config.set_value("FO[3]",value=j20)

            
            config.set_value("c12",c12)
            config.set_value("c13",c13)
            config.set_value("c14",c14)
            config.set_value("c15",c15)
            config.set_value("c16",c16)
            config.set_value("c17",c17)
            config.set_value("s3",s3)
            config.set_value("robot_type3", robot_type3)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def robot_type_update(self,com_flag = None):
        try:
            gcode = f"$v\r\n"
            if com_flag == 1:
                self.serial_com1.write(gcode.encode('utf-8'))
                time.sleep(0.1)
                data = self.serial_com1.read_all() # 从串口读取数据
                recive = str(data,"UTF-8")
                # print("receive com1:" + recive)
            
            if "Mirobot" in recive:
                self.robot_type_number = 1
            elif "E4" in recive:
                self.robot_type_number = 2

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def robot_type_update_2(self,com_flag = None):
        try:
            gcode = f"$v\r\n"
            if com_flag == 2:
                self.serial_com2.write(gcode.encode('utf-8'))
                time.sleep(0.1)
                data = self.serial_com2.read_all() # 从串口读取数据
                recive = str(data,"UTF-8")
                # print("receive com2:" + recive)
            
            if "Mirobot" in recive:
                self.robot_type_number_2 = 1
            elif "E4" in recive:
                self.robot_type_number_2 = 2

        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def robot_type_update_3(self,com_flag = None):
        try:
            gcode = f"$v\r\n"
            if com_flag == 3:
                self.serial_com3.write(gcode.encode('utf-8'))
                time.sleep(0.1)
                data = self.serial_com3.read_all() # 从串口读取数据
                recive = str(data,"UTF-8")
                # print("receive com3:" + recive)
            
            if "Mirobot" in recive:
                self.robot_type_number_3 = 1
            elif "E4" in recive:
                self.robot_type_number_3 = 2

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ### 将浮点数打包成32位2进制数据
    def float2bytes(self,f):
        b=struct.pack('<f',f)        
        return b[0],b[1],b[2],b[3]  

    def send_msg(self, message, wait_ok=None, wait_idle=None , terminator=os.linesep, com_flag = None):
        try:
            # 自动添加换行符
            if not message.endswith(terminator):
                message += terminator
            # print(f"send_msg wait_idle is {wait_idle}")
            # 串口发送数据，编码为utf-8
            if com_flag == 1 :
                if self.serial_com1 is not None:
                    self.serial_com1.write(message.encode('utf-8'))
                
            elif com_flag == 2:
                if self.serial_com2 is not None:
                    self.serial_com2.write(message.encode('utf-8'))
            
            elif com_flag == 3:
                if self.serial_com3 is not None:
                    self.serial_com3.write(message.encode('utf-8'))
            
            if wait_ok is None:
                wait_ok = False
           
            if wait_idle is None:
                wait_idle = False

            # if wait_ok:
            #     output = self.wait_for_ok()
            
            if wait_idle:
                self.wait_until_idle(com_data = com_flag)
  
        except Exception as e:
            # 日志写入串口设备写入异常
            robot_logger.error(e,exc_info=True)
            return False

    def wait_for_ok(self, reset_expected=False, disable_debug=False):  
        """等待ok到来"""  
        output = ['']  # 初始化output为空列表  
        ok_eols = ['ok']  
        reset_strings = ['Using reset pos!']  
  
        def matches_eol_strings(terms, s):  
            for eol in terms:  
                if s.endswith(eol) or eol in s:  
                    return True  
            return False  
  
        if reset_expected:  
            eols = ok_eols + reset_strings  
        else:  
            eols = ok_eols  
  
        eol_threshold = 1  # 无论在哪个操作系统下，我们统一ok的返回次数为1  
  
        eol_counter = 0  
        while eol_counter < eol_threshold:  
            msg = self.serial_device.readline(timeout=0.1)  
            if self._debug and not disable_debug and len(msg) > 0:  
                self.logger.debug(f"[RECV] {msg.decode().strip()}")  # 假设msg是bytes，需要解码并去除尾随空格  
  
            # if 'error' in msg.decode():  
            #     self.logger.error(MirobotError(msg.decode().replace('error: ', '').strip()))  
  
            # if 'ALARM' in msg.decode():  
            #     alarm_message = msg.decode().split('ALARM: ', 1)[1].strip()  
            #     self.logger.error(MirobotAlarm(alarm_message))  
  
            output.append(msg)  
  
            # if not reset_expected and matches_eol_strings(reset_strings, msg.decode()):  
            #     self.logger.error(MirobotReset('Mirobot was unexpectedly reset!'))  
  
            if matches_eol_strings(eols, msg.decode()):  
                eol_counter += 1  
  
        return [o.decode().strip() for o in output if o]  # 去除空字符串并解码

    def wait_until_idle(self, com_data = None, refresh_rate = 0.1):
        try:
            time.sleep(0.2)
            # print(f"self.status_global_3['state'] is {self.status_global_3['state']}")
            if com_data == 1:
                while self.status_global['state'] != "Idle":
                    # print(f"wait_untile idle 1")
                    time.sleep(refresh_rate)
                    # continue
            elif com_data == 2:
                while self.status_global_['state'] != "Idle":
                    # print(f"wait_untile idle 2")
                    time.sleep(refresh_rate)
                    # continue
            elif com_data == 3:
                while self.status_global_3['state'] != "Idle":
                    # print(f"wait_untile idle 3")
                    time.sleep(refresh_rate)
                    # continue

        except Exception as e:
            print(e)
    
    #### 机器人Gcode运动控制指令
    def pump_suction(self):  
        '''气泵吸气'''  
        self.set_air_pump(AIR_PUMP_SUCTION_PWM_VALUE)  
  
    def pump_blowing(self):  
        '''气泵吹气'''  
        self.set_air_pump(AIR_PUMP_BLOWING_PWM_VALUE)  
  
    def pump_on(self, is_suction=True):  
        """  
        气泵开启, 吸气/吹气  
        """  
        if is_suction:
            print(f"---pump on---")
            self.set_air_pump(AIR_PUMP_SUCTION_PWM_VALUE)  
        else:  
            self.set_air_pump(AIR_PUMP_BLOWING_PWM_VALUE)  
  
    def pump_off(self):  
        """  
        气泵关闭, 电磁阀开启, 放气  
        """  
        self.set_air_pump(AIR_PUMP_OFF_PWM_VALUE, wait_ok=False)  
        self.set_valve(VALVE_ON_PWM_VALUE, wait_ok=False)  
        time.sleep(1)  
        self.set_valve(VALVE_OFF_PWM_VALUE, wait_ok=False)  
  
    def set_air_pump(self, pwm, wait_ok=True):  
        '''设置气泵的PWM信号'''  
        if pwm not in self.pump_pwm_values:  
            robot_logger.exception(ValueError(f'pwm must be one of these values: {self.pump_pwm_values}. Was given {pwm}.'))  
            pwm = AIR_PUMP_OFF_PWM_VALUE  
        msg = f'M3S{pwm}' 
        print(f"---set_air_pump---") 
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True, com_flag=3)  
  
    def set_valve(self, pwm, wait_ok=True):  
        '''设置电磁阀的PWM'''  
        if pwm not in self.valve_pwm_values:  
            robot_logger.exception(ValueError(f'pwm must be one of these values: {self.valve_pwm_values}. Was given {pwm}.'))  
            pwm = VALVE_OFF_PWM_VALUE  
        msg = f'M4E{pwm}'  
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True, com_flag=3)  

    def home(self, has_slider=False):
        '''机械臂Homing'''
        if has_slider:
            return self.home_7axis()
        else:
            print("home")
            return self.home_6axis()

    def home_1axis(self, axis_id):
        '''单轴Homing'''
        if not isinstance(axis_id, int) or not (axis_id >= 1 and axis_id <= 7):
            return False
        msg = f'$h{axis_id}'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)

    def home_6axis(self):
        '''六轴Homing'''
        msg = f'$h'
        return self.send_msg(msg, wait_ok=False, wait_idle=True, com_flag = 1)

    def home_6axis_in_turn(self):
        '''六轴Homing, 各关节依次Homing'''
        msg = f'$hh'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)
    
    def stop(self):
        '''6轴暂停'''
        # msg = f'o117'
        # self.send_msg(msg, wait_ok=False, wait_idle=False)
        # time.sleep(0.5)
        msg = f'%'
        return self.send_msg(msg, wait_ok=False, wait_idle=False)

    def home_7axis(self):
        '''七轴Homing(本体 + 滑台)'''
        msg = f'$h0'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)
        
    def unlock_all_axis(self):
        '''解锁各轴锁定状态'''
        msg = 'M50'
        return self.send_msg(msg, wait_ok=True, wait_idle=True)
        
    def go_to_zero(self, com_Num):
        '''回零-运动到名义上的各轴零点'''
        msg = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000'
        if com_Num == 1:
            self.send_msg(msg, wait_ok=True, wait_idle=True, com_flag = 1)
        elif com_Num == 2:
            self.send_msg(msg, wait_ok=True, wait_idle=True, com_flag = 2)
        elif com_Num == 3:
            self.send_msg(msg, wait_ok=True, wait_idle=True, com_flag = 3)
        return True

    def set_speed(self, speed):
        '''设置转速'''
        # 转换为整数
        speed = int(speed)
        # 检查数值范围是否合法
        if speed <= 0 or speed > 6000:
            robot_logger.error(f"Illegal movement speed {speed}")
            return False
        # 发送指令
        msg = f'F{speed}'
        return self.send_msg(msg, wait_ok=None, wait_idle=None)

    def set_hard_limit(self, enable):
        '''
        开启硬件限位
        '''
        msg = f'$21={int(enable)}'
        return self.send_msg(msg, var_command=True, wait_ok=None)

    def set_soft_limit(self, enable):
        '''开启软限位
        注: 请谨慎使用
        '''
        msg = f'$20={int(enable)}'
        return self.send_msg(msg, var_command=True, wait_ok=None)

    def format_float_value(self, value):
        if value is None:
            return value
        if isinstance(value, float):
            # 精确到小数点后两位数
            return round(value , 2)
        else:
            return value
    
    def generate_args_string(self, instruction, pairings):
        '''生成参数字符'''
        args = [f'{arg_key}{self.format_float_value(value)}' for arg_key, value in pairings.items() if value is not None]

        return ' '.join([instruction] + args)

    def move(self, P, speed = None ,coordinate= 1, is_relative=0, mode =0, wait_ok=None):

        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        instruction = 'M20 G90 G1'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        m1 = "M20"             # 世界坐标系
        pairings = {'X': P[0], 'Y': P[1], 'Z': P[2], 'A': P[3], 'B': P[4], 'C': P[5], 'D':P[6], 'F': speed}
        if coordinate ==0:
            m1 = "M21"      # 轴角坐标系
        m2 = "G90"
        if is_relative:
            m2 = 'G91'       
        m3 ="G00"
        if mode == 1:
            m3 = "G01"

        instruction = f"{m1} {m2} {m3} "
        
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True, com_flag = 1)

    def p2p_interpolation(self, x=None, y=None, z=None, a=None, b=None, c=None, com_Num= None, speed=None, is_relative=False, is_M = False, wait_ok=None):
        '''点到点插补'''
        if is_M:
            instruction = 'M20 G90 G0'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
            if is_relative:
                instruction = 'M20 G91 G0'
        else:
            instruction = 'M21 G90 G0'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
            if is_relative:
                instruction = 'M21 G91 G0' 

        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        pairings = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        if com_Num == 1:
            return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True, com_flag = 1)
        elif com_Num == 2:
            return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True, com_flag = 2)
        elif com_Num == 3:
            return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True, com_flag = 3)

    def circular_interpolation(self, ex, ey, radius, is_cw=True, speed=None, wait_ok=None):
        '''圆弧插补
        在XY平面上, 从当前点运动到相对坐标(ex, ey).半径为radius
        `is_cw`决定圆弧是顺时针还是逆时针.
        '''
        # 判断是否合法
        distance = math.sqrt(ex**2 + ey**2)
        if distance > (radius * 2):
            self.logger.error(f'circular interpolation error, target posi is too far')
            return False

        instruction = None
        if is_cw:
            instruction = 'M20 G91 G02'
        else:
            instruction = 'M20 G91 G03'
        
        pairings = {'X': ex, 'Y': ey, 'R': radius, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True)

    def set_door_lift_distance(self, lift_distance):
        '''设置门式轨迹规划抬起的高度'''
        msg = f"$49={lift_distance}"
        return self.send_msg(msg, wait_ok=True, wait_idle=True)

    def door_interpolation(self, x=None, y=None, z=None, a=None, b=None, c=None, speed=None, is_relative=False, com_Num = None, wait_ok=None):
        '''门式插补'''
        instruction = 'M20 G90 G05'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        if is_relative:
            instruction = 'M20 G91 G05'
        
        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        pairings = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        if com_Num == 2:
            self.send_msg(msg, wait_idle=True, com_flag=2)
        elif com_Num == 3:
            self.send_msg(msg, wait_idle=True, com_flag=3)
        else:
            self.send_msg(msg, wait_idle=True, com_flag=1)
        return True

    def set_joint_angle(self, joint_angles=None, com_P=None,  speed=None, is_relative=False, com_Num = None):
        '''
        设置机械臂关节的角度
        joint_angles 目标关节角度字典, key是关节的ID号, value是角度(单位°)
            举例: {1:45.0, 2:-30.0}
        '''
        if joint_angles is not None:
            for joint_i in range(1, 8):
                # 补齐缺失的角度
                if joint_i not in joint_angles:
                    joint_angles[joint_i] = None
        elif com_P is not None:
            joint_angles = {1:com_P[0],2:com_P[1],3:com_P[2],4:com_P[3],5:com_P[4],6:com_P[5], 7:com_P[12]}
            time.sleep(0.01)
        else:
            for joint_i in range(1, 7):
                joint_angles[joint_i] = None

        return self.go_to_axis(x=joint_angles[1], y=joint_angles[2], z=joint_angles[3], a=joint_angles[4], \
            b=joint_angles[5], c=joint_angles[6], d = joint_angles[7], is_relative=is_relative, speed=speed, com_Num = com_Num,)

    def go_to_axis(self, x=None, y=None, z=None, a=None, b=None, c=None, d = None,speed=None, is_relative=False, com_Num = None,):
        '''设置关节角度/位置'''
        instruction = 'M21 G90'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        if is_relative:
            instruction = 'M21 G91'
        if not speed:
            speed = self.default_speed
        if speed:

            speed = int(speed)

        pairings = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c, 'D': d, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        if com_Num == 2:
            self.send_msg(msg, wait_idle=True, com_flag=2)
        elif com_Num == 3:
            self.send_msg(msg, wait_idle=True, com_flag=3)
        elif com_Num == 1:
            self.send_msg(msg, wait_idle=True, com_flag=1)

        return True
    
    def linear_interpolation(self, P1, speed=None, is_relative=False, com_Num = None, wait_idle=None):
        self.linear_interpolation_o(x=P1[6], y=P1[7], z=P1[8], a=P1[9], b=P1[10], c=P1[11],
                            speed=speed,is_relative=is_relative)
    
    def linear_interpolation_o(self, x=None, y=None, z=None, a=None, b=None, c=None, speed=None, is_relative=False, com_Num = None):
        '''直线插补'''
        instruction = 'M20 G90 G1'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        if is_relative:
            instruction = 'M20 G91 G1'
        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        pairings = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        if com_Num == 2:
            self.send_msg(message= msg, wait_idle=True, com_flag=2)
        elif com_Num == 3:
            self.send_msg(message= msg, wait_idle=True, com_flag=3)
        else:
            self.send_msg(message= msg, wait_idle=True, com_flag=1)
        return True

    def home_position(self, com_Num = None):
        msg = f'$h'
        if com_Num == 1:
            self.send_msg(msg, wait_idle=True, com_flag = 1)
        elif com_Num == 2:
            self.send_msg(msg, wait_idle=True, com_flag = 2)
        elif com_Num == 3:
            self.send_msg(msg, wait_idle=True, com_flag = 3)
        return True
    
    ### 复位---台面控制
    def robot_home(self, com_Num = None):
        msg = f'$h'
        if com_Num == 1:
            self.send_msg(msg,  wait_idle=False, com_flag = 1)
        elif com_Num == 2:
            self.send_msg(msg,  wait_idle=False, com_flag = 2)
        elif com_Num == 3:
            self.send_msg(msg, wait_ok=False, wait_idle=True, com_flag = 3)
        return True

    def allRobot_stop(self):
        '''全部6轴缓慢暂停'''
        msg = f'o117'
        self.send_msg(msg, wait_idle=False, com_flag = 1)
        self.send_msg(msg, wait_idle=False, com_flag = 2)
        self.send_msg(msg, wait_idle=False, com_flag = 3)

    def robot_stop(self, com_Num = None):
        '''6轴暂停'''
        msg = f'o117'
        if com_Num == 1:
            self.send_msg(msg, wait_idle=False, com_flag = 1)
        elif com_Num == 2:
            self.send_msg(msg, wait_idle=False, com_flag = 2)
        elif com_Num == 3:
            self.send_msg(msg, wait_idle=False, com_flag = 3)
        time.sleep(0.5)
        msg = f'%'
        if com_Num == 1:
            self.send_msg(msg, wait_idle=False, com_flag = 1)
        elif com_Num == 2:
            self.send_msg(msg, wait_idle=False, com_flag = 2)
        elif com_Num == 3:
            self.send_msg(msg, wait_idle=False, com_flag = 3)
        return True

    def serial_stop(self):
        if self.serial_com1:
            self.serial_com1.close()
        if self.serial_com2:
            self.serial_com2.close()
        if self.serial_com3:
            self.serial_com3.close()
        return True

    def connect(self):
        self._init_serial()
